#include "System.h"


AutoNVParameters MyAutoNVParameters;
AutoNVStateVariables MyAutoNVStateVariables;

BYTE IRFloor,IRCeiling,IRSignalStrength;

const FIXED_7_8 Wir [16] = { -45<<4, -39<<4, -33<<4, -27<<4, -21<<4, -15<<4, -9<<4, -3<<4, 3<<4, 9<<4, 15<<4, 21<<4, 27<<4, 33<<4, 39<<4, 45<<4};

FIXED_7_8 Sir_Magnitude;
FIXED_7_8 S_dot_W;

FIXED_7_8 SteeringIntegral;

#define AUTO_NV_IDLE        0x00
#define AUTO_NV_GO          0x01
#define AUTO_NV_START_DELAY 0x02
#define AUTO_NV_STOP_DELAY  0x03 


#define MAX_CENTROID_FILTER_TAPS  32

FIXED_7_8 CentroidFilterHistory[MAX_CENTROID_FILTER_TAPS];
BYTE CentroidFilterIndex = 0;
SIGNED_DWORD TempFilterSum;
FIXED_7_8 LastCentroid = 0;

#define CENTROID_DERIVATIVE_DEADZONE  ((FIXED_7_8)(0x10))

#define DEFAULT_MIN_ALLOWABLE_IR_SIGNAL_STRENGTH  50
#define DEFAULT_MAX_ALLOWABLE_IR_SIGNAL_FLOOR     50
#define DEFAULT_MAX_ALLOWABLE_IR_FAULT_TICKS      50

BYTE IRFaultTicker;

void ResetCentroidFilter()
{
 BYTE i;
 
 for(i=0;i<MAX_CENTROID_FILTER_TAPS;i++)
 {
   CentroidFilterHistory[i] = 0;
 }

  CentroidFilterIndex = 0;

}

void InitAutoNV()
{
  MyAutoNVStateVariables.IRCentroid = 0;  
  MyAutoNVStateVariables.AutoNVState = AUTO_NV_IDLE;
  CentroidFilterIndex = 0;
  
  MyAutoNVParameters.CentroidFilterLength = 0;
  MyAutoNVParameters.MinAllowableIRSignalStrength = DEFAULT_MIN_ALLOWABLE_IR_SIGNAL_STRENGTH;
  MyAutoNVParameters.MaxAllowableSignalFloor = DEFAULT_MAX_ALLOWABLE_IR_SIGNAL_FLOOR;
  MyAutoNVParameters.IRFaultTickLimit= DEFAULT_MAX_ALLOWABLE_IR_FAULT_TICKS ;
  
  
  MyAutoNVParameters.SteeringDifferentialGain  = 0;
  MyAutoNVParameters.SteeringIntegralGain = 0;	
  
  InitButtonDetect();
  IRFaultTicker = 0;
  LastCentroid = 0;
}

void CalculateCentroid()
{
    BYTE i;
    FIXED_7_8 TempCentroid;
   
   //*********************************************************************
   //Calculated the actual centroid  
   //*********************************************************************
     
     LastCentroid = MyAutoNVStateVariables.IRCentroid;
     
     Sir_Magnitude = 0;
     S_dot_W = 0;
       
     for(i=0;i<16;i++)
     {
       Sir_Magnitude += (FIXED_7_8)(CurrentSensorData->IR[i]);
       S_dot_W += MUL_FIXED_7_8_to_FIXED_7_8((FIXED_7_8)(CurrentSensorData->IR[i]),Wir[i]);
     }
     
     MyAutoNVStateVariables.IRCentroid = DIV_FIXED_7_8_by_FIXED_7_8(S_dot_W,Sir_Magnitude); 

     //*********************************************************************
     //Calculated the Centroid Derivative
     //*********************************************************************

     TempCentroid = MyAutoNVStateVariables.IRCentroid - LastCentroid; 

     if((TempCentroid < CENTROID_DERIVATIVE_DEADZONE)  && (TempCentroid>-CENTROID_DERIVATIVE_DEADZONE))
     {
        MyAutoNVStateVariables.IRCentroidDerivative = 0;
     } else
     {
         MyAutoNVStateVariables.IRCentroidDerivative = TempCentroid;
     } 

    //*********************************************************************
    //Calculated the filtered centroid (lossy, forgetful integrator)
    //*********************************************************************
    
    if(MyAutoNVParameters.CentroidFilterLength <= 1)
    {
        MyAutoNVStateVariables.IRCentroidFiltered =  0;
    
    } 
    else
    {
       TempFilterSum = 0;
      
       CentroidFilterHistory[CentroidFilterIndex] = MyAutoNVStateVariables.IRCentroid;
       CentroidFilterIndex++;
       
       if(CentroidFilterIndex == MyAutoNVParameters.CentroidFilterLength)
       {
         CentroidFilterIndex = 0;
       } 
       
       for(i = 0;  i < MyAutoNVParameters.CentroidFilterLength ; i++)
       {
         TempFilterSum  +=  (SIGNED_DWORD)CentroidFilterHistory[i];
       }
       
        MyAutoNVStateVariables.IRCentroidFiltered = (FIXED_7_8)( TempFilterSum /  (SIGNED_DWORD)MyAutoNVParameters.CentroidFilterLength);
    }  
}

void CalculateSignalStrength()
{
  
    BYTE i;
    
    IRFloor = 0xFF;
    IRCeiling = 0x00;
    
    for(i=0;i<16 ; i++)
    {
     if(CurrentSensorData->IR[i] < IRFloor)
      IRFloor = CurrentSensorData->IR[i];
     
     if(CurrentSensorData->IR[i] > IRCeiling)
      IRCeiling = CurrentSensorData->IR[i];
    }
  
    IRSignalStrength = IRCeiling - IRFloor; 
}

void AutoNV()
{

  if(ControlSystemTick == TRUE)
  {
  
    ControlSystemTick = FALSE;
     LED2_OFF;
     switch(MyAutoNVStateVariables.AutoNVState)
     {
      default:  
      case AUTO_NV_IDLE:
      
        if(SW4_PRESSED == TRUE)
        {
            SW4_PRESSED = FALSE; 
            MyAutoNVStateVariables.AutoNVState = AUTO_NV_START_DELAY;
            AUTO_NV_START_TIMER = 0;
            SetDriveServo(0);
            SetSteeringServo(0);    
        }
        
      break;

      case AUTO_NV_START_DELAY:
      
        if(AUTO_NV_START_TIMER > TWO_SECONDS)
        {
            MyAutoNVStateVariables.AutoNVState = AUTO_NV_GO;
            IRFaultTicker = 0;
        }
        
      break;        
      
      case AUTO_NV_GO:
      
      if(SW4_PRESSED == TRUE)
        {
            SW4_PRESSED = FALSE; 
            MyAutoNVStateVariables.AutoNVState = AUTO_NV_STOP_DELAY;
            AUTO_NV_START_TIMER = 0;
            SetDriveServo(0);
        }
        
      CalculateSignalStrength();
      CalculateCentroid();
      
      //************************************************************************************************************
      //Stop Logic
      //
      //Put Stop Logic (if off track) here***************
      //IF the IR signal stats are out of range,  increment a tick do not allow Servos to change/reset until
      //ticker reaches a max threshold or IR signal is OK
      
      
      if(IRSignalStrength < MyAutoNVParameters.MinAllowableIRSignalStrength
         ||  IRFloor>MyAutoNVParameters.MaxAllowableSignalFloor)
      {
         IRFaultTicker++;
         
         if(IRFaultTicker >=  MyAutoNVParameters.IRFaultTickLimit && MyAutoNVParameters.IRFaultTickLimit>0)
          {
          
           ////Move into the Delay/Idle State
             MyAutoNVStateVariables.AutoNVState = AUTO_NV_STOP_DELAY;
            AUTO_NV_START_TIMER = 0;
            SetDriveServo(0);
          }
    }
      
      //************************************************************************************************************
      //Lap Count Logic
      //
      //Put Lap Count Logic here***************
      
      
      
      //**************************************************************************************************
      //**************************************************************************************************
      //We only calculate new drive an servo commands if there isn't an IR error or
      //We arent' counting laps
      //
      //The IRCentroid should range from about -3 + 3  for for a maximum
      //signal strength.
      //
      
      //The Servo control value ranges from about -622 to 622 (integer) which is about -2.4 to 2.4  (FIXED_7_8)
      //Steering Control Gain should be negative so wheels will go in the opposite direction of the error
      
        else
        {
        
          IRFaultTicker = 0;
            
            if(MyAutoNVParameters.DisableAutoSteering == FALSE)
            {
                 
               MyAutoNVStateVariables.CommandedSteering = MUL_FIXED_7_8_to_FIXED_7_8(MyAutoNVStateVariables.IRCentroid, MyAutoNVParameters.SteeringCtrlGain);
               
               if( MyAutoNVStateVariables.IRCentroidFiltered != 0 && MyAutoNVParameters.SteeringIntegralGain != 0)
               {
                    MyAutoNVStateVariables.CommandedSteering +=   MUL_FIXED_7_8_to_FIXED_7_8(MyAutoNVStateVariables.IRCentroidFiltered, MyAutoNVParameters.SteeringIntegralGain);
               }
               
               if( MyAutoNVStateVariables.IRCentroidDerivative != 0 && MyAutoNVParameters.SteeringDifferentialGain != 0)
               {
                    MyAutoNVStateVariables.CommandedSteering +=   MUL_FIXED_7_8_to_FIXED_7_8(MyAutoNVStateVariables.IRCentroidDerivative, MyAutoNVParameters.SteeringDifferentialGain);
               }
               
         
               SetSteeringServo(MyAutoNVStateVariables.CommandedSteering);
              
            }
            
            if(MyAutoNVParameters.DisableAutoSpeed == FALSE)
            {
              MyAutoNVStateVariables.CommandedDrive = MyAutoNVParameters.MaxDriveSpeed - MUL_FIXED_7_8_to_FIXED_7_8(abs(MyAutoNVStateVariables.IRCentroid), MyAutoNVParameters.SpeedCtrlGain);
              
              if(MyAutoNVStateVariables.CommandedDrive >0)
              {
                  SetDriveServo(MyAutoNVStateVariables.CommandedDrive);
              }
            }
            else
            {
              MyAutoNVStateVariables.CommandedDrive = MyAutoNVParameters.MaxDriveSpeed;
              if(MyAutoNVStateVariables.CommandedDrive >0)
              {
                  SetDriveServo(MyAutoNVStateVariables.CommandedDrive);
              }
              
            }  
          }          
          
          break;
          
          case AUTO_NV_STOP_DELAY:
          SetDriveServo(0);
          SetSteeringServo(0);
            if(AUTO_NV_START_TIMER > TWO_SECONDS)
            {
                MyAutoNVStateVariables.AutoNVState = AUTO_NV_IDLE;
            }
          break;
         }
  
       LED2_ON;
       ControlSystemTick = FALSE;
  }
}

